<!-- HTML header for doxygen 1.8.6-->
<!DOCTYPE html PUBLIC "-//W3C//DTD XHTML 1.0 Transitional//EN" "http://www.w3.org/TR/xhtml1/DTD/xhtml1-transitional.dtd">
<html xmlns="http://www.w3.org/1999/xhtml">
<head>
<meta http-equiv="Content-Type" content="text/xhtml;charset=UTF-8"/>
<meta http-equiv="X-UA-Compatible" content="IE=9"/>
<meta name="generator" content="Doxygen 1.8.13"/>
<title>OpenCV: Real Time pose estimation of a textured object</title>
<link href="../../opencv.ico" rel="shortcut icon" type="image/x-icon" />
<link href="../../tabs.css" rel="stylesheet" type="text/css"/>
<script type="text/javascript" src="../../jquery.js"></script>
<script type="text/javascript" src="../../dynsections.js"></script>
<script type="text/javascript" src="../../tutorial-utils.js"></script>
<link href="../../search/search.css" rel="stylesheet" type="text/css"/>
<script type="text/javascript" src="../../search/searchdata.js"></script>
<script type="text/javascript" src="../../search/search.js"></script>
<script type="text/x-mathjax-config">
  MathJax.Hub.Config({
    extensions: ["tex2jax.js", "TeX/AMSmath.js", "TeX/AMSsymbols.js"],
    jax: ["input/TeX","output/HTML-CSS"],
});
//<![CDATA[
MathJax.Hub.Config(
{
  TeX: {
      Macros: {
          matTT: [ "\\[ \\left|\\begin{array}{ccc} #1 & #2 & #3\\\\ #4 & #5 & #6\\\\ #7 & #8 & #9 \\end{array}\\right| \\]", 9],
          fork: ["\\left\\{ \\begin{array}{l l} #1 & \\mbox{#2}\\\\ #3 & \\mbox{#4}\\\\ \\end{array} \\right.", 4],
          forkthree: ["\\left\\{ \\begin{array}{l l} #1 & \\mbox{#2}\\\\ #3 & \\mbox{#4}\\\\ #5 & \\mbox{#6}\\\\ \\end{array} \\right.", 6],
          forkfour: ["\\left\\{ \\begin{array}{l l} #1 & \\mbox{#2}\\\\ #3 & \\mbox{#4}\\\\ #5 & \\mbox{#6}\\\\ #7 & \\mbox{#8}\\\\ \\end{array} \\right.", 8],
          vecthree: ["\\begin{bmatrix} #1\\\\ #2\\\\ #3 \\end{bmatrix}", 3],
          vecthreethree: ["\\begin{bmatrix} #1 & #2 & #3\\\\ #4 & #5 & #6\\\\ #7 & #8 & #9 \\end{bmatrix}", 9],
          cameramatrix: ["#1 = \\begin{bmatrix} f_x & 0 & c_x\\\\ 0 & f_y & c_y\\\\ 0 & 0 & 1 \\end{bmatrix}", 1],
          distcoeffs: ["(k_1, k_2, p_1, p_2[, k_3[, k_4, k_5, k_6 [, s_1, s_2, s_3, s_4[, \\tau_x, \\tau_y]]]]) \\text{ of 4, 5, 8, 12 or 14 elements}"],
          distcoeffsfisheye: ["(k_1, k_2, k_3, k_4)"],
          hdotsfor: ["\\dots", 1],
          mathbbm: ["\\mathbb{#1}", 1],
          bordermatrix: ["\\matrix{#1}", 1]
      }
  }
}
);
//]]>
</script><script type="text/javascript" src="https://cdnjs.cloudflare.com/ajax/libs/mathjax/2.7.0/MathJax.js"></script>
<link href="../../doxygen.css" rel="stylesheet" type="text/css" />
<link href="../../stylesheet.css" rel="stylesheet" type="text/css"/>
</head>
<body>
<div id="top"><!-- do not remove this div, it is closed by doxygen! -->
<div id="titlearea">
<!--#include virtual="/google-search.html"-->
<table cellspacing="0" cellpadding="0">
 <tbody>
 <tr style="height: 56px;">
  <td id="projectlogo"><img alt="Logo" src="../../opencv-logo-small.png"/></td>
  <td style="padding-left: 0.5em;">
   <div id="projectname">OpenCV
   &#160;<span id="projectnumber">4.5.2</span>
   </div>
   <div id="projectbrief">Open Source Computer Vision</div>
  </td>
 </tr>
 </tbody>
</table>
</div>
<!-- end header part -->
<!-- Generated by Doxygen 1.8.13 -->
<script type="text/javascript">
var searchBox = new SearchBox("searchBox", "../../search",false,'Search');
</script>
<script type="text/javascript" src="../../menudata.js"></script>
<script type="text/javascript" src="../../menu.js"></script>
<script type="text/javascript">
$(function() {
  initMenu('../../',true,false,'search.php','Search');
  $(document).ready(function() { init_search(); });
});
</script>
<div id="main-nav"></div>
<!-- window showing the filter options -->
<div id="MSearchSelectWindow"
     onmouseover="return searchBox.OnSearchSelectShow()"
     onmouseout="return searchBox.OnSearchSelectHide()"
     onkeydown="return searchBox.OnSearchSelectKey(event)">
</div>

<!-- iframe showing the search results (closed by default) -->
<div id="MSearchResultsWindow">
<iframe src="javascript:void(0)" frameborder="0" 
        name="MSearchResults" id="MSearchResults">
</iframe>
</div>

<div id="nav-path" class="navpath">
  <ul>
<li class="navelem"><a class="el" href="../../d9/df8/tutorial_root.html">OpenCV Tutorials</a></li><li class="navelem"><a class="el" href="../../d6/d55/tutorial_table_of_content_calib3d.html">Camera calibration and 3D reconstruction (calib3d module)</a></li>  </ul>
</div>
</div><!-- top -->
<div class="header">
  <div class="headertitle">
<div class="title">Real Time pose estimation of a textured object </div>  </div>
</div><!--header-->
<div class="contents">
<div class="textblock"><p><b>Prev Tutorial:</b> <a class="el" href="../../d4/d94/tutorial_camera_calibration.html">Camera calibration With OpenCV</a></p>
<p><b>Next Tutorial:</b> <a class="el" href="../../d7/d21/tutorial_interactive_calibration.html">Interactive camera calibration application</a></p>
<table class="doxtable">
<tr>
<th align="right"></th><th align="left"></th></tr>
<tr>
<td align="right">Original author </td><td align="left">Edgar Riba </td></tr>
<tr>
<td align="right">Compatibility </td><td align="left">OpenCV &gt;= 3.0 </td></tr>
</table>
<p>Nowadays, augmented reality is one of the top research topic in computer vision and robotics fields. The most elemental problem in augmented reality is the estimation of the camera pose respect of an object in the case of computer vision area to do later some 3D rendering or in the case of robotics obtain an object pose in order to grasp it and do some manipulation. However, this is not a trivial problem to solve due to the fact that the most common issue in image processing is the computational cost of applying a lot of algorithms or mathematical operations for solving a problem which is basic and immediately for humans.</p>
<h2>Goal </h2>
<p>In this tutorial is explained how to build a real time application to estimate the camera pose in order to track a textured object with six degrees of freedom given a 2D image and its 3D textured model.</p>
<p>The application will have the following parts:</p>
<ul>
<li>Read 3D textured object model and object mesh.</li>
<li>Take input from Camera or Video.</li>
<li>Extract ORB features and descriptors from the scene.</li>
<li>Match scene descriptors with model descriptors using Flann matcher.</li>
<li>Pose estimation using PnP + Ransac.</li>
<li>Linear Kalman Filter for bad poses rejection.</li>
</ul>
<h2>Theory </h2>
<p>In computer vision estimate the camera pose from <em>n</em> 3D-to-2D point correspondences is a fundamental and well understood problem. The most general version of the problem requires estimating the six degrees of freedom of the pose and five calibration parameters: focal length, principal point, aspect ratio and skew. It could be established with a minimum of 6 correspondences, using the well known Direct Linear Transform (DLT) algorithm. There are, though, several simplifications to the problem which turn into an extensive list of different algorithms that improve the accuracy of the DLT.</p>
<p>The most common simplification is to assume known calibration parameters which is the so-called Perspective-*n*-Point problem:</p>
<div class="image">
<img src="../../pnp.jpg" alt="pnp.jpg"/>
</div>
<p><b>Problem Formulation:</b> Given a set of correspondences between 3D points \(p_i\) expressed in a world reference frame, and their 2D projections \(u_i\) onto the image, we seek to retrieve the pose ( \(R\) and \(t\)) of the camera w.r.t. the world and the focal length \(f\).</p>
<p>OpenCV provides four different approaches to solve the Perspective-*n*-Point problem which return \(R\) and \(t\). Then, using the following formula it's possible to project 3D points into the image plane:</p>
<p class="formulaDsp">
\[s\ \left [ \begin{matrix} u \\ v \\ 1 \end{matrix} \right ] = \left [ \begin{matrix} f_x &amp; 0 &amp; c_x \\ 0 &amp; f_y &amp; c_y \\ 0 &amp; 0 &amp; 1 \end{matrix} \right ] \left [ \begin{matrix} r_{11} &amp; r_{12} &amp; r_{13} &amp; t_1 \\ r_{21} &amp; r_{22} &amp; r_{23} &amp; t_2 \\ r_{31} &amp; r_{32} &amp; r_{33} &amp; t_3 \end{matrix} \right ] \left [ \begin{matrix} X \\ Y \\ Z\\ 1 \end{matrix} \right ]\]
</p>
<p>The complete documentation of how to manage with this equations is in <a class="el" href="../../d9/d0c/group__calib3d.html">Camera Calibration and 3D Reconstruction</a> .</p>
<h2>Source code </h2>
<p>You can find the source code of this tutorial in the <code>samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/</code> folder of the OpenCV source library.</p>
<p>The tutorial consists of two main programs:</p>
<ol type="1">
<li><p class="startli"><b>Model registration</b></p>
<p class="startli">This application is exclusive to whom don't have a 3D textured model of the object to be detected. You can use this program to create your own textured 3D model. This program only works for planar objects, then if you want to model an object with complex shape you should use a sophisticated software to create it.</p>
<p class="startli">The application needs an input image of the object to be registered and its 3D mesh. We have also to provide the intrinsic parameters of the camera with which the input image was taken. All the files need to be specified using the absolute path or the relative one from your application’s working directory. If none files are specified the program will try to open the provided default parameters.</p>
<p class="startli">The application starts up extracting the ORB features and descriptors from the input image and then uses the mesh along with the <a href="http://http://en.wikipedia.org/wiki/M%C3%B6ller%E2%80%93Trumbore_intersection_algorithm/">Möller–Trumbore intersection algorithm</a> to compute the 3D coordinates of the found features. Finally, the 3D points and the descriptors are stored in different lists in a file with YAML format which each row is a different point. The technical background on how to store the files can be found in the <a class="el" href="../../dd/d74/tutorial_file_input_output_with_xml_yml.html">File Input and Output using XML and YAML files</a> tutorial.</p>
<div class="image">
<img src="../../registration.png" alt="registration.png"/>
</div>
</li>
<li><p class="startli"><b>Model detection</b></p>
<p class="startli">The aim of this application is estimate in real time the object pose given its 3D textured model.</p>
<p class="startli">The application starts up loading the 3D textured model in YAML file format with the same structure explained in the model registration program. From the scene, the ORB features and descriptors are detected and extracted. Then, is used <a class="el" href="../../dc/de2/classcv_1_1FlannBasedMatcher.html">cv::FlannBasedMatcher</a> with <a class="el" href="../../db/d18/classcv_1_1flann_1_1GenericIndex.html">cv::flann::GenericIndex</a> to do the matching between the scene descriptors and the model descriptors. Using the found matches along with <a class="el" href="../../d9/d0c/group__calib3d.html#ga50620f0e26e02caa2e9adc07b5fbf24e">cv::solvePnPRansac</a> function the <code>R</code> and <code>t</code> of the camera are computed. Finally, a KalmanFilter is applied in order to reject bad poses.</p>
<p class="startli">In the case that you compiled OpenCV with the samples, you can find it in opencv/build/bin/cpp-tutorial-pnp_detection`. Then you can run the application and change some parameters: </p><div class="fragment"><div class="line">This program shows how to detect an <span class="keywordtype">object</span> given its 3D textured model. You can choose to use a recorded video or the webcam.</div><div class="line">Usage:</div><div class="line">  ./cpp-tutorial-pnp_detection -help</div><div class="line">Keys:</div><div class="line">  <span class="stringliteral">&#39;esc&#39;</span> - to quit.</div><div class="line">--------------------------------------------------------------------------</div><div class="line"></div><div class="line">Usage: cpp-tutorial-pnp_detection [<a class="code" href="../../d1/dae/namespacecv_1_1gapi_1_1ie.html#a3ab1729bcaf2d08e30dd2bc645410908">params</a>]</div><div class="line"></div><div class="line">  -c, --confidence (value:0.95)</div><div class="line">      <a class="code" href="../../d9/d0c/group__calib3d.html#gga58a3bc75c7337534ea9c1697b928cae0a724159df258a5d7e29410a6a2f4e6c87">RANSAC</a> confidence</div><div class="line">  -e, --<a class="code" href="../../db/de0/group__core__utils.html#gacbd081fdb20423a63cf731569ba70b2b">error</a> (value:2.0)</div><div class="line">      <a class="code" href="../../d9/d0c/group__calib3d.html#gga58a3bc75c7337534ea9c1697b928cae0a724159df258a5d7e29410a6a2f4e6c87">RANSAC</a> reprojection <a class="code" href="../../db/de0/group__core__utils.html#gacbd081fdb20423a63cf731569ba70b2b">error</a></div><div class="line">  -f, --fast (value:<span class="keyword">true</span>)</div><div class="line">      use of robust fast match</div><div class="line">  -h, --help (value:<span class="keyword">true</span>)</div><div class="line">      <a class="code" href="../../df/d57/namespacecv_1_1dnn.html#a701210a0203f2786cbfd04b2bd56da47">print</a> <span class="keyword">this</span> message</div><div class="line">  --in, --inliers (value:30)</div><div class="line">      minimum inliers <span class="keywordflow">for</span> Kalman update</div><div class="line">  --it, --iterations (value:500)</div><div class="line">      <a class="code" href="../../d9/d0c/group__calib3d.html#gga58a3bc75c7337534ea9c1697b928cae0a724159df258a5d7e29410a6a2f4e6c87">RANSAC</a> maximum iterations count</div><div class="line">  -k, --keypoints (value:2000)</div><div class="line">      number of keypoints to detect</div><div class="line">  --mesh</div><div class="line">      path to ply mesh</div><div class="line">  --method, --pnp (value:0)</div><div class="line">      PnP method: (0) <a class="code" href="../../df/d5b/group__fuzzy.html#ggaf609444993d5dcfe2d1837f80e6f2e88a42216ed5e5593d09ba0a26576061c33f">ITERATIVE</a> - (1) EPNP - (2) P3P - (3) DLS</div><div class="line">  --model</div><div class="line">      path to yml model</div><div class="line">  -r, --ratio (value:0.7)</div><div class="line">      <a class="code" href="../../d7/d1b/group__imgproc__misc.html#gae8a4a146d1ca78c626a53577199e9c57">threshold</a> for ratio test</div><div class="line">  -v, --video</div><div class="line">      path to recorded video</div></div><!-- fragment --><p> For example, you can run the application changing the pnp method: </p><div class="fragment"><div class="line">./cpp-tutorial-pnp_detection --method=2</div></div><!-- fragment --></li>
</ol>
<h2>Explanation </h2>
<p>Here is explained in detail the code for the real time application:</p>
<ol type="1">
<li><p class="startli"><b>Read 3D textured object model and object mesh.</b></p>
<p class="startli">In order to load the textured model I implemented the <em>class</em> <b>Model</b> which has the function <em>load()</em> that opens a YAML file and take the stored 3D points with its corresponding descriptors. You can find an example of a 3D textured model in <code>samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/Data/cookies_ORB.yml</code>. </p><div class="fragment"><div class="line"><span class="comment">/* Load a YAML file using OpenCV */</span></div><div class="line"><span class="keywordtype">void</span> Model::load(<span class="keyword">const</span> std::string path)</div><div class="line">{</div><div class="line">    <a class="code" href="../../d3/d63/classcv_1_1Mat.html">cv::Mat</a> points3d_mat;</div><div class="line"></div><div class="line">    <a class="code" href="../../da/d56/classcv_1_1FileStorage.html">cv::FileStorage</a> storage(path, <a class="code" href="../../da/d56/classcv_1_1FileStorage.html#a973e41cb75ef6230412a567723b7482daa0dcc459c4d100c35ccfa9a643827bed">cv::FileStorage::READ</a>);</div><div class="line">    storage[<span class="stringliteral">&quot;points_3d&quot;</span>] &gt;&gt; points3d_mat;</div><div class="line">    storage[<span class="stringliteral">&quot;descriptors&quot;</span>] &gt;&gt; descriptors_;</div><div class="line"></div><div class="line">    points3d_mat.<a class="code" href="../../d3/d63/classcv_1_1Mat.html#a33fd5d125b4c302b0c9aa86980791a77">copyTo</a>(list_points3d_in_);</div><div class="line"></div><div class="line">    storage.release();</div><div class="line"></div><div class="line">}</div></div><!-- fragment --><p> In the main program the model is loaded as follows: </p><div class="fragment"><div class="line">Model model;               <span class="comment">// instantiate Model object</span></div><div class="line">model.load(yml_read_path); <span class="comment">// load a 3D textured object model</span></div></div><!-- fragment --><p> In order to read the model mesh I implemented a <em>class</em> <b>Mesh</b> which has a function <em>load()</em> that opens a \(*\).ply file and store the 3D points of the object and also the composed triangles. You can find an example of a model mesh in <code>samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/Data/box.ply</code>. </p><div class="fragment"><div class="line"><span class="comment">/* Load a CSV with *.ply format */</span></div><div class="line"><span class="keywordtype">void</span> Mesh::load(<span class="keyword">const</span> std::string path)</div><div class="line">{</div><div class="line"></div><div class="line">    <span class="comment">// Create the reader</span></div><div class="line">    CsvReader csvReader(path);</div><div class="line"></div><div class="line">    <span class="comment">// Clear previous data</span></div><div class="line">    list_vertex_.clear();</div><div class="line">    list_triangles_.clear();</div><div class="line"></div><div class="line">    <span class="comment">// Read from .ply file</span></div><div class="line">    csvReader.readPLY(list_vertex_, list_triangles_);</div><div class="line"></div><div class="line">    <span class="comment">// Update mesh attributes</span></div><div class="line">    num_vertexs_ = list_vertex_.size();</div><div class="line">    num_triangles_ = list_triangles_.size();</div><div class="line"></div><div class="line">}</div></div><!-- fragment --><p> In the main program the mesh is loaded as follows: </p><div class="fragment"><div class="line">Mesh mesh;                <span class="comment">// instantiate Mesh object</span></div><div class="line">mesh.load(ply_read_path); <span class="comment">// load an object mesh</span></div></div><!-- fragment --><p> You can also load different model and mesh: </p><div class="fragment"><div class="line">./cpp-tutorial-pnp_detection --mesh=/absolute_path_to_your_mesh.ply --model=/absolute_path_to_your_model.yml</div></div><!-- fragment --></li>
<li><p class="startli"><b>Take input from Camera or Video</b></p>
<p class="startli">To detect is necessary capture video. It's done loading a recorded video by passing the absolute path where it is located in your machine. In order to test the application you can find a recorded video in <code>samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/Data/box.mp4</code>. </p><div class="fragment"><div class="line"><a class="code" href="../../d8/dfe/classcv_1_1VideoCapture.html">cv::VideoCapture</a> cap;                <span class="comment">// instantiate VideoCapture</span></div><div class="line">cap.<a class="code" href="../../d8/dfe/classcv_1_1VideoCapture.html#a614a1702e15f42ede5100014ce7f48ed">open</a>(video_read_path);           <span class="comment">// open a recorded video</span></div><div class="line"></div><div class="line"><span class="keywordflow">if</span>(!cap.<a class="code" href="../../d8/dfe/classcv_1_1VideoCapture.html#a9d2ca36789e7fcfe7a7be3b328038585">isOpened</a>())                  <span class="comment">// check if we succeeded</span></div><div class="line">{</div><div class="line">   std::cout &lt;&lt; <span class="stringliteral">&quot;Could not open the camera device&quot;</span> &lt;&lt; std::endl;</div><div class="line">   <span class="keywordflow">return</span> -1;</div><div class="line">}</div></div><!-- fragment --><p> Then the algorithm is computed frame per frame: </p><div class="fragment"><div class="line"><a class="code" href="../../d3/d63/classcv_1_1Mat.html">cv::Mat</a> frame, frame_vis;</div><div class="line"></div><div class="line"><span class="keywordflow">while</span>(cap.<a class="code" href="../../d8/dfe/classcv_1_1VideoCapture.html#a473055e77dd7faa4d26d686226b292c1">read</a>(frame) &amp;&amp; <a class="code" href="../../d7/dfc/group__highgui.html#ga5628525ad33f52eab17feebcfba38bd7">cv::waitKey</a>(30) != 27)    <span class="comment">// capture frame until ESC is pressed</span></div><div class="line">{</div><div class="line"></div><div class="line">    frame_vis = frame.<a class="code" href="../../d3/d63/classcv_1_1Mat.html#adff2ea98da45eae0833e73582dd4a660">clone</a>();                     <span class="comment">// refresh visualisation frame</span></div><div class="line"></div><div class="line">    <span class="comment">// MAIN ALGORITHM</span></div><div class="line"></div><div class="line">}</div></div><!-- fragment --><p> You can also load different recorded video: </p><div class="fragment"><div class="line">./cpp-tutorial-pnp_detection --video=/absolute_path_to_your_video.mp4</div></div><!-- fragment --></li>
<li><p class="startli"><b>Extract ORB features and descriptors from the scene</b></p>
<p class="startli">The next step is to detect the scene features and extract it descriptors. For this task I implemented a <em>class</em> <b>RobustMatcher</b> which has a function for keypoints detection and features extraction. You can find it in <code>samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/RobusMatcher.cpp</code>. In your <em>RobusMatch</em> object you can use any of the 2D features detectors of OpenCV. In this case I used <a class="el" href="../../db/d95/classcv_1_1ORB.html">cv::ORB</a> features because is based on <a class="el" href="../../d5/d51/group__features2d__main.html#ga816d870cbdca71c6790c71bdf17df099">cv::FAST</a> to detect the keypoints and <a class="el" href="../../d1/d93/classcv_1_1xfeatures2d_1_1BriefDescriptorExtractor.html" title="Class for computing BRIEF descriptors described in  . ">cv::xfeatures2d::BriefDescriptorExtractor</a> to extract the descriptors which means that is fast and robust to rotations. You can find more detailed information about <em>ORB</em> in the documentation.</p>
<p class="startli">The following code is how to instantiate and set the features detector and the descriptors extractor: </p><div class="fragment"><div class="line">RobustMatcher rmatcher;                                                          <span class="comment">// instantiate RobustMatcher</span></div><div class="line"></div><div class="line"><a class="code" href="../../d0/d13/classcv_1_1Feature2D.html">cv::FeatureDetector</a> * detector = <span class="keyword">new</span> cv::OrbFeatureDetector(numKeyPoints);       <span class="comment">// instantiate ORB feature detector</span></div><div class="line"><a class="code" href="../../d0/d13/classcv_1_1Feature2D.html">cv::DescriptorExtractor</a> * extractor = <span class="keyword">new</span> cv::OrbDescriptorExtractor();          <span class="comment">// instantiate ORB descriptor extractor</span></div><div class="line"></div><div class="line">rmatcher.setFeatureDetector(detector);                                           <span class="comment">// set feature detector</span></div><div class="line">rmatcher.setDescriptorExtractor(extractor);                                      <span class="comment">// set descriptor extractor</span></div></div><!-- fragment --><p> The features and descriptors will be computed by the <em>RobustMatcher</em> inside the matching function.</p>
</li>
<li><p class="startli"><b>Match scene descriptors with model descriptors using Flann matcher</b></p>
<p class="startli">It is the first step in our detection algorithm. The main idea is to match the scene descriptors with our model descriptors in order to know the 3D coordinates of the found features into the current scene.</p>
<p class="startli">Firstly, we have to set which matcher we want to use. In this case is used <a class="el" href="../../dc/de2/classcv_1_1FlannBasedMatcher.html">cv::FlannBasedMatcher</a> matcher which in terms of computational cost is faster than the <a class="el" href="../../d3/da1/classcv_1_1BFMatcher.html">cv::BFMatcher</a> matcher as we increase the trained collection of features. Then, for FlannBased matcher the index created is <em>Multi-Probe LSH: Efficient Indexing for High-Dimensional Similarity Search</em> due to <em>ORB</em> descriptors are binary.</p>
<p class="startli">You can tune the <em>LSH</em> and search parameters to improve the matching efficiency: </p><div class="fragment"><div class="line"><a class="code" href="../../dc/d84/group__core__basic.html#ga6395ca871a678020c4a31fadf7e8cc63">cv::Ptr&lt;cv::flann::IndexParams&gt;</a> indexParams = cv::makePtr&lt;cv::flann::LshIndexParams&gt;(6, 12, 1); <span class="comment">// instantiate LSH index parameters</span></div><div class="line"><a class="code" href="../../dc/d84/group__core__basic.html#ga6395ca871a678020c4a31fadf7e8cc63">cv::Ptr&lt;cv::flann::SearchParams&gt;</a> searchParams = cv::makePtr&lt;cv::flann::SearchParams&gt;(50);       <span class="comment">// instantiate flann search parameters</span></div><div class="line"></div><div class="line"><a class="code" href="../../db/d39/classcv_1_1DescriptorMatcher.html">cv::DescriptorMatcher</a> * matcher = <span class="keyword">new</span> <a class="code" href="../../dc/de2/classcv_1_1FlannBasedMatcher.html">cv::FlannBasedMatcher</a>(indexParams, searchParams);         <span class="comment">// instantiate FlannBased matcher</span></div><div class="line">rmatcher.setDescriptorMatcher(matcher);                                                         <span class="comment">// set matcher</span></div></div><!-- fragment --><p> Secondly, we have to call the matcher by using <em>robustMatch()</em> or <em>fastRobustMatch()</em> function. The difference of using this two functions is its computational cost. The first method is slower but more robust at filtering good matches because uses two ratio test and a symmetry test. In contrast, the second method is faster but less robust because only applies a single ratio test to the matches.</p>
<p class="startli">The following code is to get the model 3D points and its descriptors and then call the matcher in the main program: </p><div class="fragment"><div class="line"><span class="comment">// Get the MODEL INFO</span></div><div class="line"></div><div class="line">std::vector&lt;cv::Point3f&gt; list_points3d_model = model.get_points3d();  <span class="comment">// list with model 3D coordinates</span></div><div class="line"><a class="code" href="../../d3/d63/classcv_1_1Mat.html">cv::Mat</a> descriptors_model = model.get_descriptors();                  <span class="comment">// list with descriptors of each 3D coordinate</span></div></div><!-- fragment --> <div class="fragment"><div class="line"><span class="comment">// -- Step 1: Robust matching between model descriptors and scene descriptors</span></div><div class="line"></div><div class="line">std::vector&lt;cv::DMatch&gt; good_matches;       <span class="comment">// to obtain the model 3D points  in the scene</span></div><div class="line">std::vector&lt;cv::KeyPoint&gt; keypoints_scene;  <span class="comment">// to obtain the 2D points of the scene</span></div><div class="line"></div><div class="line"><span class="keywordflow">if</span>(fast_match)</div><div class="line">{</div><div class="line">    rmatcher.fastRobustMatch(frame, good_matches, keypoints_scene, descriptors_model);</div><div class="line">}</div><div class="line"><span class="keywordflow">else</span></div><div class="line">{</div><div class="line">    rmatcher.robustMatch(frame, good_matches, keypoints_scene, descriptors_model);</div><div class="line">}</div></div><!-- fragment --><p> The following code corresponds to the <em>robustMatch()</em> function which belongs to the <em>RobustMatcher</em> class. This function uses the given image to detect the keypoints and extract the descriptors, match using <em>two Nearest Neighbour</em> the extracted descriptors with the given model descriptors and vice versa. Then, a ratio test is applied to the two direction matches in order to remove these matches which its distance ratio between the first and second best match is larger than a given threshold. Finally, a symmetry test is applied in order the remove non symmetrical matches. </p><div class="fragment"><div class="line"><span class="keywordtype">void</span> RobustMatcher::robustMatch( <span class="keyword">const</span> <a class="code" href="../../d3/d63/classcv_1_1Mat.html">cv::Mat</a>&amp; frame, std::vector&lt;cv::DMatch&gt;&amp; good_matches,</div><div class="line">                                 std::vector&lt;cv::KeyPoint&gt;&amp; keypoints_frame,</div><div class="line">                                 <span class="keyword">const</span> std::vector&lt;cv::KeyPoint&gt;&amp; keypoints_model, <span class="keyword">const</span> <a class="code" href="../../d3/d63/classcv_1_1Mat.html">cv::Mat</a>&amp; descriptors_model )</div><div class="line">{</div><div class="line"></div><div class="line">    <span class="comment">// 1a. Detection of the ORB features</span></div><div class="line">    this-&gt;computeKeyPoints(frame, keypoints_frame);</div><div class="line"></div><div class="line">    <span class="comment">// 1b. Extraction of the ORB descriptors</span></div><div class="line">    <a class="code" href="../../d3/d63/classcv_1_1Mat.html">cv::Mat</a> descriptors_frame;</div><div class="line">    this-&gt;computeDescriptors(frame, keypoints_frame, descriptors_frame);</div><div class="line"></div><div class="line">    <span class="comment">// 2. Match the two image descriptors</span></div><div class="line">    std::vector&lt;std::vector&lt;cv::DMatch&gt; &gt; matches12, matches21;</div><div class="line"></div><div class="line">    <span class="comment">// 2a. From image 1 to image 2</span></div><div class="line">    matcher_-&gt;knnMatch(descriptors_frame, descriptors_model, matches12, 2); <span class="comment">// return 2 nearest neighbours</span></div><div class="line"></div><div class="line">    <span class="comment">// 2b. From image 2 to image 1</span></div><div class="line">    matcher_-&gt;knnMatch(descriptors_model, descriptors_frame, matches21, 2); <span class="comment">// return 2 nearest neighbours</span></div><div class="line"></div><div class="line">    <span class="comment">// 3. Remove matches for which NN ratio is &gt; than threshold</span></div><div class="line">    <span class="comment">// clean image 1 -&gt; image 2 matches</span></div><div class="line">    <span class="keywordtype">int</span> removed1 = ratioTest(matches12);</div><div class="line">    <span class="comment">// clean image 2 -&gt; image 1 matches</span></div><div class="line">    <span class="keywordtype">int</span> removed2 = ratioTest(matches21);</div><div class="line"></div><div class="line">    <span class="comment">// 4. Remove non-symmetrical matches</span></div><div class="line">    symmetryTest(matches12, matches21, good_matches);</div><div class="line"></div><div class="line">}</div></div><!-- fragment --><p> After the matches filtering we have to subtract the 2D and 3D correspondences from the found scene keypoints and our 3D model using the obtained <em>DMatches</em> vector. For more information about <a class="el" href="../../d4/de0/classcv_1_1DMatch.html">cv::DMatch</a> check the documentation. </p><div class="fragment"><div class="line"><span class="comment">// -- Step 2: Find out the 2D/3D correspondences</span></div><div class="line"></div><div class="line">std::vector&lt;cv::Point3f&gt; list_points3d_model_match;    <span class="comment">// container for the model 3D coordinates found in the scene</span></div><div class="line">std::vector&lt;cv::Point2f&gt; list_points2d_scene_match;    <span class="comment">// container for the model 2D coordinates found in the scene</span></div><div class="line"></div><div class="line"><span class="keywordflow">for</span>(<span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> match_index = 0; match_index &lt; good_matches.size(); ++match_index)</div><div class="line">{</div><div class="line">    <a class="code" href="../../df/d6c/classcv_1_1Point3__.html">cv::Point3f</a> point3d_model = list_points3d_model[ good_matches[match_index].trainIdx ];   <span class="comment">// 3D point from model</span></div><div class="line">    <a class="code" href="../../db/d4e/classcv_1_1Point__.html">cv::Point2f</a> point2d_scene = keypoints_scene[ good_matches[match_index].queryIdx ].pt;    <span class="comment">// 2D point from the scene</span></div><div class="line">    list_points3d_model_match.push_back(point3d_model);                                      <span class="comment">// add 3D point</span></div><div class="line">    list_points2d_scene_match.push_back(point2d_scene);                                      <span class="comment">// add 2D point</span></div><div class="line">}</div></div><!-- fragment --><p> You can also change the ratio test threshold, the number of keypoints to detect as well as use or not the robust matcher: </p><div class="fragment"><div class="line">./cpp-tutorial-pnp_detection --ratio=0.8 --keypoints=1000 --fast=<span class="keyword">false</span></div></div><!-- fragment --></li>
<li><p class="startli"><b>Pose estimation using PnP + Ransac</b></p>
<p class="startli">Once with the 2D and 3D correspondences we have to apply a PnP algorithm in order to estimate the camera pose. The reason why we have to use <a class="el" href="../../d9/d0c/group__calib3d.html#ga50620f0e26e02caa2e9adc07b5fbf24e">cv::solvePnPRansac</a> instead of <a class="el" href="../../d9/d0c/group__calib3d.html#ga549c2075fac14829ff4a58bc931c033d">cv::solvePnP</a> is due to the fact that after the matching not all the found correspondences are correct and, as like as not, there are false correspondences or also called <em>outliers</em>. The <a href="http://en.wikipedia.org/wiki/RANSAC">Random Sample Consensus</a> or <em>Ransac</em> is a non-deterministic iterative method which estimate parameters of a mathematical model from observed data producing an approximate result as the number of iterations increase. After applying <em>Ransac</em> all the <em>outliers</em> will be eliminated to then estimate the camera pose with a certain probability to obtain a good solution.</p>
<p class="startli">For the camera pose estimation I have implemented a <em>class</em> <b>PnPProblem</b>. This <em>class</em> has 4 attributes: a given calibration matrix, the rotation matrix, the translation matrix and the rotation-translation matrix. The intrinsic calibration parameters of the camera which you are using to estimate the pose are necessary. In order to obtain the parameters you can check <a class="el" href="../../dc/d43/tutorial_camera_calibration_square_chess.html">Camera calibration with square chessboard</a> and <a class="el" href="../../d4/d94/tutorial_camera_calibration.html">Camera calibration With OpenCV</a> tutorials.</p>
<p class="startli">The following code is how to declare the <em>PnPProblem class</em> in the main program: </p><div class="fragment"><div class="line"><span class="comment">// Intrinsic camera parameters: UVC WEBCAM</span></div><div class="line"></div><div class="line"><span class="keywordtype">double</span> f = 55;                           <span class="comment">// focal length in mm</span></div><div class="line"><span class="keywordtype">double</span> sx = 22.3, sy = 14.9;             <span class="comment">// sensor size</span></div><div class="line"><span class="keywordtype">double</span> width = 640, height = 480;        <span class="comment">// image size</span></div><div class="line"></div><div class="line"><span class="keywordtype">double</span> params_WEBCAM[] = { width*f/sx,   <span class="comment">// fx</span></div><div class="line">                           height*f/sy,  <span class="comment">// fy</span></div><div class="line">                           width/2,      <span class="comment">// cx</span></div><div class="line">                           height/2};    <span class="comment">// cy</span></div><div class="line"></div><div class="line">PnPProblem pnp_detection(params_WEBCAM); <span class="comment">// instantiate PnPProblem class</span></div></div><!-- fragment --><p> The following code is how the <em>PnPProblem class</em> initialises its attributes: </p><div class="fragment"><div class="line"><span class="comment">// Custom constructor given the intrinsic camera parameters</span></div><div class="line"></div><div class="line">PnPProblem::PnPProblem(<span class="keyword">const</span> <span class="keywordtype">double</span> <a class="code" href="../../d1/dae/namespacecv_1_1gapi_1_1ie.html#a3ab1729bcaf2d08e30dd2bc645410908">params</a>[])</div><div class="line">{</div><div class="line">  _A_matrix = <a class="code" href="../../d3/d63/classcv_1_1Mat.html#a0b57b6a326c8876d944d188a46e0f556">cv::Mat::zeros</a>(3, 3, <a class="code" href="../../d1/d1b/group__core__hal__interface.html#ga3b256d68c6002b6afd60ab9538d717a8">CV_64FC1</a>);   <span class="comment">// intrinsic camera parameters</span></div><div class="line">  _A_matrix.at&lt;<span class="keywordtype">double</span>&gt;(0, 0) = params[0];       <span class="comment">//      [ fx   0  cx ]</span></div><div class="line">  _A_matrix.at&lt;<span class="keywordtype">double</span>&gt;(1, 1) = params[1];       <span class="comment">//      [  0  fy  cy ]</span></div><div class="line">  _A_matrix.at&lt;<span class="keywordtype">double</span>&gt;(0, 2) = params[2];       <span class="comment">//      [  0   0   1 ]</span></div><div class="line">  _A_matrix.at&lt;<span class="keywordtype">double</span>&gt;(1, 2) = params[3];</div><div class="line">  _A_matrix.at&lt;<span class="keywordtype">double</span>&gt;(2, 2) = 1;</div><div class="line">  _R_matrix = <a class="code" href="../../d3/d63/classcv_1_1Mat.html#a0b57b6a326c8876d944d188a46e0f556">cv::Mat::zeros</a>(3, 3, <a class="code" href="../../d1/d1b/group__core__hal__interface.html#ga3b256d68c6002b6afd60ab9538d717a8">CV_64FC1</a>);   <span class="comment">// rotation matrix</span></div><div class="line">  _t_matrix = <a class="code" href="../../d3/d63/classcv_1_1Mat.html#a0b57b6a326c8876d944d188a46e0f556">cv::Mat::zeros</a>(3, 1, <a class="code" href="../../d1/d1b/group__core__hal__interface.html#ga3b256d68c6002b6afd60ab9538d717a8">CV_64FC1</a>);   <span class="comment">// translation matrix</span></div><div class="line">  _P_matrix = <a class="code" href="../../d3/d63/classcv_1_1Mat.html#a0b57b6a326c8876d944d188a46e0f556">cv::Mat::zeros</a>(3, 4, <a class="code" href="../../d1/d1b/group__core__hal__interface.html#ga3b256d68c6002b6afd60ab9538d717a8">CV_64FC1</a>);   <span class="comment">// rotation-translation matrix</span></div><div class="line"></div><div class="line">}</div></div><!-- fragment --><p> OpenCV provides four PnP methods: ITERATIVE, EPNP, P3P and DLS. Depending on the application type, the estimation method will be different. In the case that we want to make a real time application, the more suitable methods are EPNP and P3P since they are faster than ITERATIVE and DLS at finding an optimal solution. However, EPNP and P3P are not especially robust in front of planar surfaces and sometimes the pose estimation seems to have a mirror effect. Therefore, in this tutorial an ITERATIVE method is used due to the object to be detected has planar surfaces.</p>
<p class="startli">The OpenCV RANSAC implementation wants you to provide three parameters: 1) the maximum number of iterations until the algorithm stops, 2) the maximum allowed distance between the observed and computed point projections to consider it an inlier and 3) the confidence to obtain a good result. You can tune these parameters in order to improve your algorithm performance. Increasing the number of iterations will have a more accurate solution, but will take more time to find a solution. Increasing the reprojection error will reduce the computation time, but your solution will be unaccurate. Decreasing the confidence your algorithm will be faster, but the obtained solution will be unaccurate.</p>
<p class="startli">The following parameters work for this application: </p><div class="fragment"><div class="line"><span class="comment">// RANSAC parameters</span></div><div class="line"></div><div class="line"><span class="keywordtype">int</span> iterationsCount = 500;        <span class="comment">// number of Ransac iterations.</span></div><div class="line"><span class="keywordtype">float</span> reprojectionError = 2.0;    <span class="comment">// maximum allowed distance to consider it an inlier.</span></div><div class="line"><span class="keywordtype">float</span> confidence = 0.95;          <span class="comment">// RANSAC successful confidence.</span></div></div><!-- fragment --><p> The following code corresponds to the <em>estimatePoseRANSAC()</em> function which belongs to the <em>PnPProblem class</em>. This function estimates the rotation and translation matrix given a set of 2D/3D correspondences, the desired PnP method to use, the output inliers container and the Ransac parameters: </p><div class="fragment"><div class="line"><span class="comment">// Estimate the pose given a list of 2D/3D correspondences with RANSAC and the method to use</span></div><div class="line"></div><div class="line"><span class="keywordtype">void</span> PnPProblem::estimatePoseRANSAC( <span class="keyword">const</span> std::vector&lt;cv::Point3f&gt; &amp;list_points3d,        <span class="comment">// list with model 3D coordinates</span></div><div class="line">                                     <span class="keyword">const</span> std::vector&lt;cv::Point2f&gt; &amp;list_points2d,        <span class="comment">// list with scene 2D coordinates</span></div><div class="line">                                     <span class="keywordtype">int</span> flags, <a class="code" href="../../d3/d63/classcv_1_1Mat.html">cv::Mat</a> &amp;inliers, <span class="keywordtype">int</span> iterationsCount,     <span class="comment">// PnP method; inliers container</span></div><div class="line">                                     <span class="keywordtype">float</span> reprojectionError, <span class="keywordtype">float</span> confidence )           <span class="comment">// RANSAC parameters</span></div><div class="line">{</div><div class="line">    <a class="code" href="../../d3/d63/classcv_1_1Mat.html">cv::Mat</a> distCoeffs = <a class="code" href="../../d3/d63/classcv_1_1Mat.html#a0b57b6a326c8876d944d188a46e0f556">cv::Mat::zeros</a>(4, 1, <a class="code" href="../../d1/d1b/group__core__hal__interface.html#ga3b256d68c6002b6afd60ab9538d717a8">CV_64FC1</a>);    <span class="comment">// vector of distortion coefficients</span></div><div class="line">    <a class="code" href="../../d3/d63/classcv_1_1Mat.html">cv::Mat</a> rvec = <a class="code" href="../../d3/d63/classcv_1_1Mat.html#a0b57b6a326c8876d944d188a46e0f556">cv::Mat::zeros</a>(3, 1, <a class="code" href="../../d1/d1b/group__core__hal__interface.html#ga3b256d68c6002b6afd60ab9538d717a8">CV_64FC1</a>);          <span class="comment">// output rotation vector</span></div><div class="line">    <a class="code" href="../../d3/d63/classcv_1_1Mat.html">cv::Mat</a> tvec = <a class="code" href="../../d3/d63/classcv_1_1Mat.html#a0b57b6a326c8876d944d188a46e0f556">cv::Mat::zeros</a>(3, 1, <a class="code" href="../../d1/d1b/group__core__hal__interface.html#ga3b256d68c6002b6afd60ab9538d717a8">CV_64FC1</a>);          <span class="comment">// output translation vector</span></div><div class="line"></div><div class="line">    <span class="keywordtype">bool</span> useExtrinsicGuess = <span class="keyword">false</span>;   <span class="comment">// if true the function uses the provided rvec and tvec values as</span></div><div class="line">                                      <span class="comment">// initial approximations of the rotation and translation vectors</span></div><div class="line"></div><div class="line">    <a class="code" href="../../d9/d0c/group__calib3d.html#ga50620f0e26e02caa2e9adc07b5fbf24e">cv::solvePnPRansac</a>( list_points3d, list_points2d, _A_matrix, distCoeffs, rvec, tvec,</div><div class="line">                        useExtrinsicGuess, iterationsCount, reprojectionError, confidence,</div><div class="line">                        inliers, flags );</div><div class="line"></div><div class="line">    <a class="code" href="../../d9/d0c/group__calib3d.html#ga61585db663d9da06b68e70cfbf6a1eac">Rodrigues</a>(rvec,_R_matrix);                   <span class="comment">// converts Rotation Vector to Matrix</span></div><div class="line">    _t_matrix = tvec;                            <span class="comment">// set translation matrix</span></div><div class="line"></div><div class="line">    this-&gt;set_P_matrix(_R_matrix, _t_matrix);    <span class="comment">// set rotation-translation matrix</span></div><div class="line"></div><div class="line">}</div></div><!-- fragment --><p> In the following code are the 3th and 4th steps of the main algorithm. The first, calling the above function and the second taking the output inliers vector from RANSAC to get the 2D scene points for drawing purpose. As seen in the code we must be sure to apply RANSAC if we have matches, in the other case, the function <a class="el" href="../../d9/d0c/group__calib3d.html#ga50620f0e26e02caa2e9adc07b5fbf24e">cv::solvePnPRansac</a> crashes due to any OpenCV <em>bug</em>. </p><div class="fragment"><div class="line"><span class="keywordflow">if</span>(good_matches.size() &gt; 0) <span class="comment">// None matches, then RANSAC crashes</span></div><div class="line">{</div><div class="line"></div><div class="line">    <span class="comment">// -- Step 3: Estimate the pose using RANSAC approach</span></div><div class="line">    pnp_detection.estimatePoseRANSAC( list_points3d_model_match, list_points2d_scene_match,</div><div class="line">                                      pnpMethod, inliers_idx, iterationsCount, reprojectionError, confidence );</div><div class="line"></div><div class="line"></div><div class="line">    <span class="comment">// -- Step 4: Catch the inliers keypoints to draw</span></div><div class="line">    <span class="keywordflow">for</span>(<span class="keywordtype">int</span> inliers_index = 0; inliers_index &lt; inliers_idx.rows; ++inliers_index)</div><div class="line">    {</div><div class="line">    <span class="keywordtype">int</span> n = inliers_idx.at&lt;<span class="keywordtype">int</span>&gt;(inliers_index);         <span class="comment">// i-inlier</span></div><div class="line">    <a class="code" href="../../db/d4e/classcv_1_1Point__.html">cv::Point2f</a> point2d = list_points2d_scene_match[n]; <span class="comment">// i-inlier point 2D</span></div><div class="line">    list_points2d_inliers.push_back(point2d);           <span class="comment">// add i-inlier to list</span></div><div class="line">}</div></div><!-- fragment --><p> Finally, once the camera pose has been estimated we can use the \(R\) and \(t\) in order to compute the 2D projection onto the image of a given 3D point expressed in a world reference frame using the showed formula on <em>Theory</em>.</p>
<p class="startli">The following code corresponds to the <em>backproject3DPoint()</em> function which belongs to the <em>PnPProblem class</em>. The function backproject a given 3D point expressed in a world reference frame onto a 2D image: </p><div class="fragment"><div class="line"><span class="comment">// Backproject a 3D point to 2D using the estimated pose parameters</span></div><div class="line"></div><div class="line"><a class="code" href="../../db/d4e/classcv_1_1Point__.html">cv::Point2f</a> PnPProblem::backproject3DPoint(<span class="keyword">const</span> <a class="code" href="../../df/d6c/classcv_1_1Point3__.html">cv::Point3f</a> &amp;point3d)</div><div class="line">{</div><div class="line">    <span class="comment">// 3D point vector [x y z 1]&#39;</span></div><div class="line">    <a class="code" href="../../d3/d63/classcv_1_1Mat.html">cv::Mat</a> point3d_vec = <a class="code" href="../../d3/d63/classcv_1_1Mat.html">cv::Mat</a>(4, 1, <a class="code" href="../../d1/d1b/group__core__hal__interface.html#ga3b256d68c6002b6afd60ab9538d717a8">CV_64FC1</a>);</div><div class="line">    point3d_vec.<a class="code" href="../../d3/d63/classcv_1_1Mat.html#aa5d20fc86d41d59e4d71ae93daee9726">at</a>&lt;<span class="keywordtype">double</span>&gt;(0) = point3d.<a class="code" href="../../df/d6c/classcv_1_1Point3__.html#a363a0ec9e4ebce7209438cb14c29ab47">x</a>;</div><div class="line">    point3d_vec.<a class="code" href="../../d3/d63/classcv_1_1Mat.html#aa5d20fc86d41d59e4d71ae93daee9726">at</a>&lt;<span class="keywordtype">double</span>&gt;(1) = point3d.<a class="code" href="../../df/d6c/classcv_1_1Point3__.html#aa332b734e658368c647744adb9065810">y</a>;</div><div class="line">    point3d_vec.<a class="code" href="../../d3/d63/classcv_1_1Mat.html#aa5d20fc86d41d59e4d71ae93daee9726">at</a>&lt;<span class="keywordtype">double</span>&gt;(2) = point3d.<a class="code" href="../../df/d6c/classcv_1_1Point3__.html#a1af9c182772a3c082e505f50eb5d2ee1">z</a>;</div><div class="line">    point3d_vec.<a class="code" href="../../d3/d63/classcv_1_1Mat.html#aa5d20fc86d41d59e4d71ae93daee9726">at</a>&lt;<span class="keywordtype">double</span>&gt;(3) = 1;</div><div class="line"></div><div class="line">    <span class="comment">// 2D point vector [u v 1]&#39;</span></div><div class="line">    <a class="code" href="../../d3/d63/classcv_1_1Mat.html">cv::Mat</a> point2d_vec = <a class="code" href="../../d3/d63/classcv_1_1Mat.html">cv::Mat</a>(4, 1, <a class="code" href="../../d1/d1b/group__core__hal__interface.html#ga3b256d68c6002b6afd60ab9538d717a8">CV_64FC1</a>);</div><div class="line">    point2d_vec = _A_matrix * _P_matrix * point3d_vec;</div><div class="line"></div><div class="line">    <span class="comment">// Normalization of [u v]&#39;</span></div><div class="line">    <a class="code" href="../../db/d4e/classcv_1_1Point__.html">cv::Point2f</a> point2d;</div><div class="line">    point2d.<a class="code" href="../../db/d4e/classcv_1_1Point__.html#a4c96fa7bdbfe390be5ed356edb274ff3">x</a> = point2d_vec.<a class="code" href="../../d3/d63/classcv_1_1Mat.html#aa5d20fc86d41d59e4d71ae93daee9726">at</a>&lt;<span class="keywordtype">double</span>&gt;(0) / point2d_vec.<a class="code" href="../../d3/d63/classcv_1_1Mat.html#aa5d20fc86d41d59e4d71ae93daee9726">at</a>&lt;<span class="keywordtype">double</span>&gt;(2);</div><div class="line">    point2d.<a class="code" href="../../db/d4e/classcv_1_1Point__.html#a157337197338ff199e5df1a393022f15">y</a> = point2d_vec.<a class="code" href="../../d3/d63/classcv_1_1Mat.html#aa5d20fc86d41d59e4d71ae93daee9726">at</a>&lt;<span class="keywordtype">double</span>&gt;(1) / point2d_vec.<a class="code" href="../../d3/d63/classcv_1_1Mat.html#aa5d20fc86d41d59e4d71ae93daee9726">at</a>&lt;<span class="keywordtype">double</span>&gt;(2);</div><div class="line"></div><div class="line">    <span class="keywordflow">return</span> point2d;</div><div class="line">}</div></div><!-- fragment --><p> The above function is used to compute all the 3D points of the object <em>Mesh</em> to show the pose of the object.</p>
<p class="startli">You can also change RANSAC parameters and PnP method: </p><div class="fragment"><div class="line">./cpp-tutorial-pnp_detection --<a class="code" href="../../db/de0/group__core__utils.html#gacbd081fdb20423a63cf731569ba70b2b">error</a>=0.25 --confidence=0.90 --iterations=250 --method=3</div></div><!-- fragment --></li>
<li><p class="startli"><b>Linear Kalman Filter for bad poses rejection</b></p>
<p class="startli">Is it common in computer vision or robotics fields that after applying detection or tracking techniques, bad results are obtained due to some sensor errors. In order to avoid these bad detections in this tutorial is explained how to implement a Linear Kalman Filter. The Kalman Filter will be applied after detected a given number of inliers.</p>
<p class="startli">You can find more information about what <a href="http://en.wikipedia.org/wiki/Kalman_filter">Kalman Filter</a> is. In this tutorial it's used the OpenCV implementation of the <a class="el" href="../../dd/d6a/classcv_1_1KalmanFilter.html">cv::KalmanFilter</a> based on <a href="http://campar.in.tum.de/Chair/KalmanFilter">Linear Kalman Filter for position and orientation tracking</a> to set the dynamics and measurement models.</p>
<p class="startli">Firstly, we have to define our state vector which will have 18 states: the positional data (x,y,z) with its first and second derivatives (velocity and acceleration), then rotation is added in form of three euler angles (roll, pitch, jaw) together with their first and second derivatives (angular velocity and acceleration)</p>
<p class="formulaDsp">
\[X = (x,y,z,\dot x,\dot y,\dot z,\ddot x,\ddot y,\ddot z,\psi,\theta,\phi,\dot \psi,\dot \theta,\dot \phi,\ddot \psi,\ddot \theta,\ddot \phi)^T\]
</p>
<p class="startli">Secondly, we have to define the number of measurements which will be 6: from \(R\) and \(t\) we can extract \((x,y,z)\) and \((\psi,\theta,\phi)\). In addition, we have to define the number of control actions to apply to the system which in this case will be <em>zero</em>. Finally, we have to define the differential time between measurements which in this case is \(1/T\), where <em>T</em> is the frame rate of the video. </p><div class="fragment"><div class="line"><a class="code" href="../../dd/d6a/classcv_1_1KalmanFilter.html">cv::KalmanFilter</a> KF;         <span class="comment">// instantiate Kalman Filter</span></div><div class="line"></div><div class="line"><span class="keywordtype">int</span> nStates = 18;            <span class="comment">// the number of states</span></div><div class="line"><span class="keywordtype">int</span> nMeasurements = 6;       <span class="comment">// the number of measured states</span></div><div class="line"><span class="keywordtype">int</span> nInputs = 0;             <span class="comment">// the number of action control</span></div><div class="line"></div><div class="line"><span class="keywordtype">double</span> dt = 0.125;           <span class="comment">// time between measurements (1/FPS)</span></div><div class="line"></div><div class="line">initKalmanFilter(KF, nStates, nMeasurements, nInputs, dt);    <span class="comment">// init function</span></div></div><!-- fragment --><p> The following code corresponds to the <em>Kalman Filter</em> initialisation. Firstly, is set the process noise, the measurement noise and the error covariance matrix. Secondly, are set the transition matrix which is the dynamic model and finally the measurement matrix, which is the measurement model.</p>
<p class="startli">You can tune the process and measurement noise to improve the <em>Kalman Filter</em> performance. As the measurement noise is reduced the faster will converge doing the algorithm sensitive in front of bad measurements. </p><div class="fragment"><div class="line"><span class="keywordtype">void</span> initKalmanFilter(<a class="code" href="../../dd/d6a/classcv_1_1KalmanFilter.html">cv::KalmanFilter</a> &amp;KF, <span class="keywordtype">int</span> nStates, <span class="keywordtype">int</span> nMeasurements, <span class="keywordtype">int</span> nInputs, <span class="keywordtype">double</span> dt)</div><div class="line">{</div><div class="line"></div><div class="line">  KF.<a class="code" href="../../dd/d6a/classcv_1_1KalmanFilter.html#a4f136c39c016d3530c7c5801dd1ddb3b">init</a>(nStates, nMeasurements, nInputs, <a class="code" href="../../d1/d1b/group__core__hal__interface.html#ga30a562691cc5987bc88eb7bb7a8faf2b">CV_64F</a>);                 <span class="comment">// init Kalman Filter</span></div><div class="line"></div><div class="line">  <a class="code" href="../../d2/de8/group__core__array.html#ga388d7575224a4a277ceb98ccaa327c99">cv::setIdentity</a>(KF.<a class="code" href="../../dd/d6a/classcv_1_1KalmanFilter.html#af19be9c0630d0f658bdbaea409a35cda">processNoiseCov</a>, <a class="code" href="../../d1/da0/classcv_1_1Scalar__.html#ac1509a4b8454fe7fe29db069e13a2e6f">cv::Scalar::all</a>(1e-5));       <span class="comment">// set process noise</span></div><div class="line">  <a class="code" href="../../d2/de8/group__core__array.html#ga388d7575224a4a277ceb98ccaa327c99">cv::setIdentity</a>(KF.<a class="code" href="../../dd/d6a/classcv_1_1KalmanFilter.html#a828d051035ba807966ad65edf288a08e">measurementNoiseCov</a>, <a class="code" href="../../d1/da0/classcv_1_1Scalar__.html#ac1509a4b8454fe7fe29db069e13a2e6f">cv::Scalar::all</a>(1e-4));   <span class="comment">// set measurement noise</span></div><div class="line">  <a class="code" href="../../d2/de8/group__core__array.html#ga388d7575224a4a277ceb98ccaa327c99">cv::setIdentity</a>(KF.<a class="code" href="../../dd/d6a/classcv_1_1KalmanFilter.html#a446d8e9a0105b0aa35cd66119c529803">errorCovPost</a>, <a class="code" href="../../d1/da0/classcv_1_1Scalar__.html#ac1509a4b8454fe7fe29db069e13a2e6f">cv::Scalar::all</a>(1));             <span class="comment">// error covariance</span></div><div class="line"></div><div class="line"></div><div class="line">                 <span class="comment">/* DYNAMIC MODEL */</span></div><div class="line"></div><div class="line">  <span class="comment">//  [1 0 0 dt  0  0 dt2   0   0 0 0 0  0  0  0   0   0   0]</span></div><div class="line">  <span class="comment">//  [0 1 0  0 dt  0   0 dt2   0 0 0 0  0  0  0   0   0   0]</span></div><div class="line">  <span class="comment">//  [0 0 1  0  0 dt   0   0 dt2 0 0 0  0  0  0   0   0   0]</span></div><div class="line">  <span class="comment">//  [0 0 0  1  0  0  dt   0   0 0 0 0  0  0  0   0   0   0]</span></div><div class="line">  <span class="comment">//  [0 0 0  0  1  0   0  dt   0 0 0 0  0  0  0   0   0   0]</span></div><div class="line">  <span class="comment">//  [0 0 0  0  0  1   0   0  dt 0 0 0  0  0  0   0   0   0]</span></div><div class="line">  <span class="comment">//  [0 0 0  0  0  0   1   0   0 0 0 0  0  0  0   0   0   0]</span></div><div class="line">  <span class="comment">//  [0 0 0  0  0  0   0   1   0 0 0 0  0  0  0   0   0   0]</span></div><div class="line">  <span class="comment">//  [0 0 0  0  0  0   0   0   1 0 0 0  0  0  0   0   0   0]</span></div><div class="line">  <span class="comment">//  [0 0 0  0  0  0   0   0   0 1 0 0 dt  0  0 dt2   0   0]</span></div><div class="line">  <span class="comment">//  [0 0 0  0  0  0   0   0   0 0 1 0  0 dt  0   0 dt2   0]</span></div><div class="line">  <span class="comment">//  [0 0 0  0  0  0   0   0   0 0 0 1  0  0 dt   0   0 dt2]</span></div><div class="line">  <span class="comment">//  [0 0 0  0  0  0   0   0   0 0 0 0  1  0  0  dt   0   0]</span></div><div class="line">  <span class="comment">//  [0 0 0  0  0  0   0   0   0 0 0 0  0  1  0   0  dt   0]</span></div><div class="line">  <span class="comment">//  [0 0 0  0  0  0   0   0   0 0 0 0  0  0  1   0   0  dt]</span></div><div class="line">  <span class="comment">//  [0 0 0  0  0  0   0   0   0 0 0 0  0  0  0   1   0   0]</span></div><div class="line">  <span class="comment">//  [0 0 0  0  0  0   0   0   0 0 0 0  0  0  0   0   1   0]</span></div><div class="line">  <span class="comment">//  [0 0 0  0  0  0   0   0   0 0 0 0  0  0  0   0   0   1]</span></div><div class="line"></div><div class="line">  <span class="comment">// position</span></div><div class="line">  KF.<a class="code" href="../../dd/d6a/classcv_1_1KalmanFilter.html#a0657173e411acbf40d2d3c6b46e03b19">transitionMatrix</a>.<a class="code" href="../../d3/d63/classcv_1_1Mat.html#aa5d20fc86d41d59e4d71ae93daee9726">at</a>&lt;<span class="keywordtype">double</span>&gt;(0,3) = dt;</div><div class="line">  KF.<a class="code" href="../../dd/d6a/classcv_1_1KalmanFilter.html#a0657173e411acbf40d2d3c6b46e03b19">transitionMatrix</a>.<a class="code" href="../../d3/d63/classcv_1_1Mat.html#aa5d20fc86d41d59e4d71ae93daee9726">at</a>&lt;<span class="keywordtype">double</span>&gt;(1,4) = dt;</div><div class="line">  KF.<a class="code" href="../../dd/d6a/classcv_1_1KalmanFilter.html#a0657173e411acbf40d2d3c6b46e03b19">transitionMatrix</a>.<a class="code" href="../../d3/d63/classcv_1_1Mat.html#aa5d20fc86d41d59e4d71ae93daee9726">at</a>&lt;<span class="keywordtype">double</span>&gt;(2,5) = dt;</div><div class="line">  KF.<a class="code" href="../../dd/d6a/classcv_1_1KalmanFilter.html#a0657173e411acbf40d2d3c6b46e03b19">transitionMatrix</a>.<a class="code" href="../../d3/d63/classcv_1_1Mat.html#aa5d20fc86d41d59e4d71ae93daee9726">at</a>&lt;<span class="keywordtype">double</span>&gt;(3,6) = dt;</div><div class="line">  KF.<a class="code" href="../../dd/d6a/classcv_1_1KalmanFilter.html#a0657173e411acbf40d2d3c6b46e03b19">transitionMatrix</a>.<a class="code" href="../../d3/d63/classcv_1_1Mat.html#aa5d20fc86d41d59e4d71ae93daee9726">at</a>&lt;<span class="keywordtype">double</span>&gt;(4,7) = dt;</div><div class="line">  KF.<a class="code" href="../../dd/d6a/classcv_1_1KalmanFilter.html#a0657173e411acbf40d2d3c6b46e03b19">transitionMatrix</a>.<a class="code" href="../../d3/d63/classcv_1_1Mat.html#aa5d20fc86d41d59e4d71ae93daee9726">at</a>&lt;<span class="keywordtype">double</span>&gt;(5,8) = dt;</div><div class="line">  KF.<a class="code" href="../../dd/d6a/classcv_1_1KalmanFilter.html#a0657173e411acbf40d2d3c6b46e03b19">transitionMatrix</a>.<a class="code" href="../../d3/d63/classcv_1_1Mat.html#aa5d20fc86d41d59e4d71ae93daee9726">at</a>&lt;<span class="keywordtype">double</span>&gt;(0,6) = 0.5*<a class="code" href="../../d7/dcc/group__core__utils__softfloat.html#ga8bc36646a43b82baa15f151a973fb0c5">pow</a>(dt,2);</div><div class="line">  KF.<a class="code" href="../../dd/d6a/classcv_1_1KalmanFilter.html#a0657173e411acbf40d2d3c6b46e03b19">transitionMatrix</a>.<a class="code" href="../../d3/d63/classcv_1_1Mat.html#aa5d20fc86d41d59e4d71ae93daee9726">at</a>&lt;<span class="keywordtype">double</span>&gt;(1,7) = 0.5*<a class="code" href="../../d7/dcc/group__core__utils__softfloat.html#ga8bc36646a43b82baa15f151a973fb0c5">pow</a>(dt,2);</div><div class="line">  KF.<a class="code" href="../../dd/d6a/classcv_1_1KalmanFilter.html#a0657173e411acbf40d2d3c6b46e03b19">transitionMatrix</a>.<a class="code" href="../../d3/d63/classcv_1_1Mat.html#aa5d20fc86d41d59e4d71ae93daee9726">at</a>&lt;<span class="keywordtype">double</span>&gt;(2,8) = 0.5*<a class="code" href="../../d7/dcc/group__core__utils__softfloat.html#ga8bc36646a43b82baa15f151a973fb0c5">pow</a>(dt,2);</div><div class="line"></div><div class="line">  <span class="comment">// orientation</span></div><div class="line">  KF.<a class="code" href="../../dd/d6a/classcv_1_1KalmanFilter.html#a0657173e411acbf40d2d3c6b46e03b19">transitionMatrix</a>.<a class="code" href="../../d3/d63/classcv_1_1Mat.html#aa5d20fc86d41d59e4d71ae93daee9726">at</a>&lt;<span class="keywordtype">double</span>&gt;(9,12) = dt;</div><div class="line">  KF.<a class="code" href="../../dd/d6a/classcv_1_1KalmanFilter.html#a0657173e411acbf40d2d3c6b46e03b19">transitionMatrix</a>.<a class="code" href="../../d3/d63/classcv_1_1Mat.html#aa5d20fc86d41d59e4d71ae93daee9726">at</a>&lt;<span class="keywordtype">double</span>&gt;(10,13) = dt;</div><div class="line">  KF.<a class="code" href="../../dd/d6a/classcv_1_1KalmanFilter.html#a0657173e411acbf40d2d3c6b46e03b19">transitionMatrix</a>.<a class="code" href="../../d3/d63/classcv_1_1Mat.html#aa5d20fc86d41d59e4d71ae93daee9726">at</a>&lt;<span class="keywordtype">double</span>&gt;(11,14) = dt;</div><div class="line">  KF.<a class="code" href="../../dd/d6a/classcv_1_1KalmanFilter.html#a0657173e411acbf40d2d3c6b46e03b19">transitionMatrix</a>.<a class="code" href="../../d3/d63/classcv_1_1Mat.html#aa5d20fc86d41d59e4d71ae93daee9726">at</a>&lt;<span class="keywordtype">double</span>&gt;(12,15) = dt;</div><div class="line">  KF.<a class="code" href="../../dd/d6a/classcv_1_1KalmanFilter.html#a0657173e411acbf40d2d3c6b46e03b19">transitionMatrix</a>.<a class="code" href="../../d3/d63/classcv_1_1Mat.html#aa5d20fc86d41d59e4d71ae93daee9726">at</a>&lt;<span class="keywordtype">double</span>&gt;(13,16) = dt;</div><div class="line">  KF.<a class="code" href="../../dd/d6a/classcv_1_1KalmanFilter.html#a0657173e411acbf40d2d3c6b46e03b19">transitionMatrix</a>.<a class="code" href="../../d3/d63/classcv_1_1Mat.html#aa5d20fc86d41d59e4d71ae93daee9726">at</a>&lt;<span class="keywordtype">double</span>&gt;(14,17) = dt;</div><div class="line">  KF.<a class="code" href="../../dd/d6a/classcv_1_1KalmanFilter.html#a0657173e411acbf40d2d3c6b46e03b19">transitionMatrix</a>.<a class="code" href="../../d3/d63/classcv_1_1Mat.html#aa5d20fc86d41d59e4d71ae93daee9726">at</a>&lt;<span class="keywordtype">double</span>&gt;(9,15) = 0.5*<a class="code" href="../../d7/dcc/group__core__utils__softfloat.html#ga8bc36646a43b82baa15f151a973fb0c5">pow</a>(dt,2);</div><div class="line">  KF.<a class="code" href="../../dd/d6a/classcv_1_1KalmanFilter.html#a0657173e411acbf40d2d3c6b46e03b19">transitionMatrix</a>.<a class="code" href="../../d3/d63/classcv_1_1Mat.html#aa5d20fc86d41d59e4d71ae93daee9726">at</a>&lt;<span class="keywordtype">double</span>&gt;(10,16) = 0.5*<a class="code" href="../../d7/dcc/group__core__utils__softfloat.html#ga8bc36646a43b82baa15f151a973fb0c5">pow</a>(dt,2);</div><div class="line">  KF.<a class="code" href="../../dd/d6a/classcv_1_1KalmanFilter.html#a0657173e411acbf40d2d3c6b46e03b19">transitionMatrix</a>.<a class="code" href="../../d3/d63/classcv_1_1Mat.html#aa5d20fc86d41d59e4d71ae93daee9726">at</a>&lt;<span class="keywordtype">double</span>&gt;(11,17) = 0.5*<a class="code" href="../../d7/dcc/group__core__utils__softfloat.html#ga8bc36646a43b82baa15f151a973fb0c5">pow</a>(dt,2);</div><div class="line"></div><div class="line"></div><div class="line">       <span class="comment">/* MEASUREMENT MODEL */</span></div><div class="line"></div><div class="line">  <span class="comment">//  [1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]</span></div><div class="line">  <span class="comment">//  [0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]</span></div><div class="line">  <span class="comment">//  [0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]</span></div><div class="line">  <span class="comment">//  [0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0]</span></div><div class="line">  <span class="comment">//  [0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0]</span></div><div class="line">  <span class="comment">//  [0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0]</span></div><div class="line"></div><div class="line">  KF.<a class="code" href="../../dd/d6a/classcv_1_1KalmanFilter.html#a0f60b78726d8eccf74a1f2479c2d1f97">measurementMatrix</a>.<a class="code" href="../../d3/d63/classcv_1_1Mat.html#aa5d20fc86d41d59e4d71ae93daee9726">at</a>&lt;<span class="keywordtype">double</span>&gt;(0,0) = 1;  <span class="comment">// x</span></div><div class="line">  KF.<a class="code" href="../../dd/d6a/classcv_1_1KalmanFilter.html#a0f60b78726d8eccf74a1f2479c2d1f97">measurementMatrix</a>.<a class="code" href="../../d3/d63/classcv_1_1Mat.html#aa5d20fc86d41d59e4d71ae93daee9726">at</a>&lt;<span class="keywordtype">double</span>&gt;(1,1) = 1;  <span class="comment">// y</span></div><div class="line">  KF.<a class="code" href="../../dd/d6a/classcv_1_1KalmanFilter.html#a0f60b78726d8eccf74a1f2479c2d1f97">measurementMatrix</a>.<a class="code" href="../../d3/d63/classcv_1_1Mat.html#aa5d20fc86d41d59e4d71ae93daee9726">at</a>&lt;<span class="keywordtype">double</span>&gt;(2,2) = 1;  <span class="comment">// z</span></div><div class="line">  KF.<a class="code" href="../../dd/d6a/classcv_1_1KalmanFilter.html#a0f60b78726d8eccf74a1f2479c2d1f97">measurementMatrix</a>.<a class="code" href="../../d3/d63/classcv_1_1Mat.html#aa5d20fc86d41d59e4d71ae93daee9726">at</a>&lt;<span class="keywordtype">double</span>&gt;(3,9) = 1;  <span class="comment">// roll</span></div><div class="line">  KF.<a class="code" href="../../dd/d6a/classcv_1_1KalmanFilter.html#a0f60b78726d8eccf74a1f2479c2d1f97">measurementMatrix</a>.<a class="code" href="../../d3/d63/classcv_1_1Mat.html#aa5d20fc86d41d59e4d71ae93daee9726">at</a>&lt;<span class="keywordtype">double</span>&gt;(4,10) = 1; <span class="comment">// pitch</span></div><div class="line">  KF.<a class="code" href="../../dd/d6a/classcv_1_1KalmanFilter.html#a0f60b78726d8eccf74a1f2479c2d1f97">measurementMatrix</a>.<a class="code" href="../../d3/d63/classcv_1_1Mat.html#aa5d20fc86d41d59e4d71ae93daee9726">at</a>&lt;<span class="keywordtype">double</span>&gt;(5,11) = 1; <span class="comment">// yaw</span></div><div class="line"></div><div class="line">}</div></div><!-- fragment --><p> In the following code is the 5th step of the main algorithm. When the obtained number of inliers after <em>Ransac</em> is over the threshold, the measurements matrix is filled and then the <em>Kalman Filter</em> is updated: </p><div class="fragment"><div class="line"><span class="comment">// -- Step 5: Kalman Filter</span></div><div class="line"></div><div class="line"><span class="comment">// GOOD MEASUREMENT</span></div><div class="line"><span class="keywordflow">if</span>( inliers_idx.rows &gt;= minInliersKalman )</div><div class="line">{</div><div class="line"></div><div class="line">    <span class="comment">// Get the measured translation</span></div><div class="line">    <a class="code" href="../../d3/d63/classcv_1_1Mat.html">cv::Mat</a> translation_measured(3, 1, <a class="code" href="../../d1/d1b/group__core__hal__interface.html#ga30a562691cc5987bc88eb7bb7a8faf2b">CV_64F</a>);</div><div class="line">    translation_measured = pnp_detection.get_t_matrix();</div><div class="line"></div><div class="line">    <span class="comment">// Get the measured rotation</span></div><div class="line">    <a class="code" href="../../d3/d63/classcv_1_1Mat.html">cv::Mat</a> rotation_measured(3, 3, <a class="code" href="../../d1/d1b/group__core__hal__interface.html#ga30a562691cc5987bc88eb7bb7a8faf2b">CV_64F</a>);</div><div class="line">    rotation_measured = pnp_detection.get_R_matrix();</div><div class="line"></div><div class="line">    <span class="comment">// fill the measurements vector</span></div><div class="line">    fillMeasurements(measurements, translation_measured, rotation_measured);</div><div class="line"></div><div class="line">}</div><div class="line"></div><div class="line"><span class="comment">// Instantiate estimated translation and rotation</span></div><div class="line"><a class="code" href="../../d3/d63/classcv_1_1Mat.html">cv::Mat</a> translation_estimated(3, 1, <a class="code" href="../../d1/d1b/group__core__hal__interface.html#ga30a562691cc5987bc88eb7bb7a8faf2b">CV_64F</a>);</div><div class="line"><a class="code" href="../../d3/d63/classcv_1_1Mat.html">cv::Mat</a> rotation_estimated(3, 3, <a class="code" href="../../d1/d1b/group__core__hal__interface.html#ga30a562691cc5987bc88eb7bb7a8faf2b">CV_64F</a>);</div><div class="line"></div><div class="line"><span class="comment">// update the Kalman filter with good measurements</span></div><div class="line">updateKalmanFilter( KF, measurements,</div><div class="line">              translation_estimated, rotation_estimated);</div></div><!-- fragment --><p> The following code corresponds to the <em>fillMeasurements()</em> function which converts the measured <a href="http://euclideanspace.com/maths/geometry/rotations/conversions/matrixToEuler/index.htm">Rotation Matrix to Eulers angles</a> and fill the measurements matrix along with the measured translation vector: </p><div class="fragment"><div class="line"><span class="keywordtype">void</span> fillMeasurements( <a class="code" href="../../d3/d63/classcv_1_1Mat.html">cv::Mat</a> &amp;measurements,</div><div class="line">                   <span class="keyword">const</span> <a class="code" href="../../d3/d63/classcv_1_1Mat.html">cv::Mat</a> &amp;translation_measured, <span class="keyword">const</span> <a class="code" href="../../d3/d63/classcv_1_1Mat.html">cv::Mat</a> &amp;rotation_measured)</div><div class="line">{</div><div class="line">    <span class="comment">// Convert rotation matrix to euler angles</span></div><div class="line">    <a class="code" href="../../d3/d63/classcv_1_1Mat.html">cv::Mat</a> measured_eulers(3, 1, <a class="code" href="../../d1/d1b/group__core__hal__interface.html#ga30a562691cc5987bc88eb7bb7a8faf2b">CV_64F</a>);</div><div class="line">    measured_eulers = rot2euler(rotation_measured);</div><div class="line"></div><div class="line">    <span class="comment">// Set measurement to predict</span></div><div class="line">    measurements.<a class="code" href="../../d3/d63/classcv_1_1Mat.html#aa5d20fc86d41d59e4d71ae93daee9726">at</a>&lt;<span class="keywordtype">double</span>&gt;(0) = translation_measured.<a class="code" href="../../d3/d63/classcv_1_1Mat.html#aa5d20fc86d41d59e4d71ae93daee9726">at</a>&lt;<span class="keywordtype">double</span>&gt;(0); <span class="comment">// x</span></div><div class="line">    measurements.<a class="code" href="../../d3/d63/classcv_1_1Mat.html#aa5d20fc86d41d59e4d71ae93daee9726">at</a>&lt;<span class="keywordtype">double</span>&gt;(1) = translation_measured.<a class="code" href="../../d3/d63/classcv_1_1Mat.html#aa5d20fc86d41d59e4d71ae93daee9726">at</a>&lt;<span class="keywordtype">double</span>&gt;(1); <span class="comment">// y</span></div><div class="line">    measurements.<a class="code" href="../../d3/d63/classcv_1_1Mat.html#aa5d20fc86d41d59e4d71ae93daee9726">at</a>&lt;<span class="keywordtype">double</span>&gt;(2) = translation_measured.<a class="code" href="../../d3/d63/classcv_1_1Mat.html#aa5d20fc86d41d59e4d71ae93daee9726">at</a>&lt;<span class="keywordtype">double</span>&gt;(2); <span class="comment">// z</span></div><div class="line">    measurements.<a class="code" href="../../d3/d63/classcv_1_1Mat.html#aa5d20fc86d41d59e4d71ae93daee9726">at</a>&lt;<span class="keywordtype">double</span>&gt;(3) = measured_eulers.at&lt;<span class="keywordtype">double</span>&gt;(0);      <span class="comment">// roll</span></div><div class="line">    measurements.<a class="code" href="../../d3/d63/classcv_1_1Mat.html#aa5d20fc86d41d59e4d71ae93daee9726">at</a>&lt;<span class="keywordtype">double</span>&gt;(4) = measured_eulers.at&lt;<span class="keywordtype">double</span>&gt;(1);      <span class="comment">// pitch</span></div><div class="line">    measurements.<a class="code" href="../../d3/d63/classcv_1_1Mat.html#aa5d20fc86d41d59e4d71ae93daee9726">at</a>&lt;<span class="keywordtype">double</span>&gt;(5) = measured_eulers.at&lt;<span class="keywordtype">double</span>&gt;(2);      <span class="comment">// yaw</span></div><div class="line">}</div></div><!-- fragment --><p> The following code corresponds to the <em>updateKalmanFilter()</em> function which update the Kalman Filter and set the estimated Rotation Matrix and translation vector. The estimated Rotation Matrix comes from the estimated <a href="http://euclideanspace.com/maths/geometry/rotations/conversions/eulerToMatrix/index.htm">Euler angles to Rotation Matrix</a>. </p><div class="fragment"><div class="line"><span class="keywordtype">void</span> updateKalmanFilter( <a class="code" href="../../dd/d6a/classcv_1_1KalmanFilter.html">cv::KalmanFilter</a> &amp;KF, <a class="code" href="../../d3/d63/classcv_1_1Mat.html">cv::Mat</a> &amp;measurement,</div><div class="line">                     <a class="code" href="../../d3/d63/classcv_1_1Mat.html">cv::Mat</a> &amp;translation_estimated, <a class="code" href="../../d3/d63/classcv_1_1Mat.html">cv::Mat</a> &amp;rotation_estimated )</div><div class="line">{</div><div class="line"></div><div class="line">    <span class="comment">// First predict, to update the internal statePre variable</span></div><div class="line">    <a class="code" href="../../d3/d63/classcv_1_1Mat.html">cv::Mat</a> prediction = KF.<a class="code" href="../../dd/d6a/classcv_1_1KalmanFilter.html#aa710d2255566bec8d6ce608d103d4fa7">predict</a>();</div><div class="line"></div><div class="line">    <span class="comment">// The &quot;correct&quot; phase that is going to use the predicted value and our measurement</span></div><div class="line">    <a class="code" href="../../d3/d63/classcv_1_1Mat.html">cv::Mat</a> estimated = KF.<a class="code" href="../../dd/d6a/classcv_1_1KalmanFilter.html#a60eb7feb569222ad0657ef1875884b5e">correct</a>(measurement);</div><div class="line"></div><div class="line">    <span class="comment">// Estimated translation</span></div><div class="line">    translation_estimated.<a class="code" href="../../d3/d63/classcv_1_1Mat.html#aa5d20fc86d41d59e4d71ae93daee9726">at</a>&lt;<span class="keywordtype">double</span>&gt;(0) = estimated.<a class="code" href="../../d3/d63/classcv_1_1Mat.html#aa5d20fc86d41d59e4d71ae93daee9726">at</a>&lt;<span class="keywordtype">double</span>&gt;(0);</div><div class="line">    translation_estimated.<a class="code" href="../../d3/d63/classcv_1_1Mat.html#aa5d20fc86d41d59e4d71ae93daee9726">at</a>&lt;<span class="keywordtype">double</span>&gt;(1) = estimated.<a class="code" href="../../d3/d63/classcv_1_1Mat.html#aa5d20fc86d41d59e4d71ae93daee9726">at</a>&lt;<span class="keywordtype">double</span>&gt;(1);</div><div class="line">    translation_estimated.<a class="code" href="../../d3/d63/classcv_1_1Mat.html#aa5d20fc86d41d59e4d71ae93daee9726">at</a>&lt;<span class="keywordtype">double</span>&gt;(2) = estimated.<a class="code" href="../../d3/d63/classcv_1_1Mat.html#aa5d20fc86d41d59e4d71ae93daee9726">at</a>&lt;<span class="keywordtype">double</span>&gt;(2);</div><div class="line"></div><div class="line">    <span class="comment">// Estimated euler angles</span></div><div class="line">    <a class="code" href="../../d3/d63/classcv_1_1Mat.html">cv::Mat</a> eulers_estimated(3, 1, <a class="code" href="../../d1/d1b/group__core__hal__interface.html#ga30a562691cc5987bc88eb7bb7a8faf2b">CV_64F</a>);</div><div class="line">    eulers_estimated.at&lt;<span class="keywordtype">double</span>&gt;(0) = estimated.<a class="code" href="../../d3/d63/classcv_1_1Mat.html#aa5d20fc86d41d59e4d71ae93daee9726">at</a>&lt;<span class="keywordtype">double</span>&gt;(9);</div><div class="line">    eulers_estimated.at&lt;<span class="keywordtype">double</span>&gt;(1) = estimated.<a class="code" href="../../d3/d63/classcv_1_1Mat.html#aa5d20fc86d41d59e4d71ae93daee9726">at</a>&lt;<span class="keywordtype">double</span>&gt;(10);</div><div class="line">    eulers_estimated.at&lt;<span class="keywordtype">double</span>&gt;(2) = estimated.<a class="code" href="../../d3/d63/classcv_1_1Mat.html#aa5d20fc86d41d59e4d71ae93daee9726">at</a>&lt;<span class="keywordtype">double</span>&gt;(11);</div><div class="line"></div><div class="line">    <span class="comment">// Convert estimated quaternion to rotation matrix</span></div><div class="line">    rotation_estimated = euler2rot(eulers_estimated);</div><div class="line"></div><div class="line">}</div></div><!-- fragment --><p> The 6th step is set the estimated rotation-translation matrix: </p><div class="fragment"><div class="line"><span class="comment">// -- Step 6: Set estimated projection matrix</span></div><div class="line">pnp_detection_est.set_P_matrix(rotation_estimated, translation_estimated);</div></div><!-- fragment --><p> The last and optional step is draw the found pose. To do it I implemented a function to draw all the mesh 3D points and an extra reference axis: </p><div class="fragment"><div class="line"><span class="comment">// -- Step X: Draw pose</span></div><div class="line"></div><div class="line">drawObjectMesh(frame_vis, &amp;mesh, &amp;pnp_detection, green);                <span class="comment">// draw current pose</span></div><div class="line">drawObjectMesh(frame_vis, &amp;mesh, &amp;pnp_detection_est, yellow);           <span class="comment">// draw estimated pose</span></div><div class="line"></div><div class="line"><span class="keywordtype">double</span> l = 5;</div><div class="line">std::vector&lt;cv::Point2f&gt; pose_points2d;</div><div class="line">pose_points2d.push_back(pnp_detection_est.backproject3DPoint(<a class="code" href="../../df/d6c/classcv_1_1Point3__.html">cv::Point3f</a>(0,0,0)));    <span class="comment">// axis center</span></div><div class="line">pose_points2d.push_back(pnp_detection_est.backproject3DPoint(<a class="code" href="../../df/d6c/classcv_1_1Point3__.html">cv::Point3f</a>(l,0,0)));    <span class="comment">// axis x</span></div><div class="line">pose_points2d.push_back(pnp_detection_est.backproject3DPoint(<a class="code" href="../../df/d6c/classcv_1_1Point3__.html">cv::Point3f</a>(0,l,0)));    <span class="comment">// axis y</span></div><div class="line">pose_points2d.push_back(pnp_detection_est.backproject3DPoint(<a class="code" href="../../df/d6c/classcv_1_1Point3__.html">cv::Point3f</a>(0,0,l)));    <span class="comment">// axis z</span></div><div class="line">draw3DCoordinateAxes(frame_vis, pose_points2d);                                       <span class="comment">// draw axes</span></div></div><!-- fragment --><p> You can also modify the minimum inliers to update Kalman Filter: </p><div class="fragment"><div class="line">./cpp-tutorial-pnp_detection --inliers=20</div></div><!-- fragment --></li>
</ol>
<h2>Results </h2>
<p>The following videos are the results of pose estimation in real time using the explained detection algorithm using the following parameters: </p><div class="fragment"><div class="line"><span class="comment">// Robust Matcher parameters</span></div><div class="line"></div><div class="line"><span class="keywordtype">int</span> numKeyPoints = 2000;      <span class="comment">// number of detected keypoints</span></div><div class="line"><span class="keywordtype">float</span> ratio = 0.70f;          <span class="comment">// ratio test</span></div><div class="line"><span class="keywordtype">bool</span> fast_match = <span class="keyword">true</span>;       <span class="comment">// fastRobustMatch() or robustMatch()</span></div><div class="line"></div><div class="line"></div><div class="line"><span class="comment">// RANSAC parameters</span></div><div class="line"></div><div class="line"><span class="keywordtype">int</span> iterationsCount = 500;    <span class="comment">// number of Ransac iterations.</span></div><div class="line"><span class="keywordtype">int</span> reprojectionError = 2.0;  <span class="comment">// maximum allowed distance to consider it an inlier.</span></div><div class="line"><span class="keywordtype">float</span> confidence = 0.95;      <span class="comment">// ransac successful confidence.</span></div><div class="line"></div><div class="line"></div><div class="line"><span class="comment">// Kalman Filter parameters</span></div><div class="line"></div><div class="line"><span class="keywordtype">int</span> minInliersKalman = 30;    <span class="comment">// Kalman threshold updating</span></div></div><!-- fragment --><p> You can watch the real time pose estimation on the <a href="http://www.youtube.com/user/opencvdev/videos">YouTube here</a>.</p>
<div align='center'><iframe title='Video' width='560' height='349' src='https://www.youtube.com/embed/XNATklaJlSQ?rel=0' frameborder='0' align='middle' allowfullscreen></iframe></div> <div align='center'><iframe title='Video' width='560' height='349' src='https://www.youtube.com/embed/YLS9bWek78k?rel=0' frameborder='0' align='middle' allowfullscreen></iframe></div> </div></div><!-- contents -->
<!-- HTML footer for doxygen 1.8.6-->
<!-- start footer part -->
<hr class="footer"/><address class="footer"><small>
Generated on Fri Apr 2 2021 11:36:34 for OpenCV by &#160;<a href="http://www.doxygen.org/index.html">
<img class="footer" src="../../doxygen.png" alt="doxygen"/>
</a> 1.8.13
</small></address>
<script type="text/javascript">
//<![CDATA[
addTutorialsButtons();
//]]>
</script>
</body>
</html>
